#include "optflow.hpp"
#include "parameters.hpp"

#include <algorithm>
#include <opencv/cv.h>

#include <vector>
#include <iostream>
#include <algorithm>
using namespace std;


/////////
//
// FrameManager implementation
//
/////////


FrameSource::FrameSource()
{ }


bool FrameSource::setup(void)
{
  cam.open(0);

  if (!cam.isOpened()) {
    return false;
  }

  cam >> frame;
  clean_frame.create(frame.size(), CV_8UC1);
  prev_clean_frame.create(frame.size(), CV_8UC1);

  return true;
}


void FrameSource::update(void)
{
  cv::Mat tmp1, tmp2;
  
  cam >> frame;

  // flip horizontally (mirror effect)
  cv::flip(frame, frame, 1);

  // convert to grayscale for easier processing
  cv::cvtColor(frame, tmp1, CV_BGR2GRAY);

  // we don't need a big image
  cv::resize(tmp1, tmp2, cv::Size(), SCALE, SCALE);

  // smooth it to remove some noise
  cv::GaussianBlur(tmp2, tmp1, cv::Size(5,5), 0);

  prev_clean_frame = clean_frame;
  clean_frame = tmp2;
}


cv::Mat FrameSource::get_frame(void) const
{
  return frame;
}


cv::Mat FrameSource::get_clean_frame(void) const
{
  return clean_frame;
}


cv::Mat FrameSource::get_previous_clean_frame(void) const
{
  return prev_clean_frame;
}



/////////
//
// Calibration process
//
/////////


static void sort_pair(vector<cv::Point2f>& points)
{
  if (points[1] < points[0])
    std::swap(points[0], points[1]);
}


vector<cv::Point2f> find_hands_corners(cv::Mat& frame, cv::Point2f left_hint,
                                       cv::Point2f right_hint)
{
  cv::Size frame_size = frame.size();

  cv::Mat left_mask  = cv::square_mask(frame_size, HAND_MASK_SIZE, left_hint);
  cv::Mat right_mask = cv::square_mask(frame_size, HAND_MASK_SIZE, right_hint);

  cv::Mat total = left_mask + right_mask + frame;
  cv::imshow(DEBUG_WINDOW, total);

  vector<cv::Point2f> left_pts, right_pts, result;

  cv::goodFeaturesToTrack(
    frame, left_pts, 1,
    0.05, 10, left_mask
  );

  cv::goodFeaturesToTrack(
    frame, right_pts, 1,
    0.05, 10, right_mask
  );

  result.push_back(left_pts[0]);
  result.push_back(right_pts[0]);;

  cv::findCornerSubPix(
    frame, result,
    cv::Size(LK_WIN_SIZE, LK_WIN_SIZE),
    cv::Size(-1, -1),
    cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)
  );

  sort_pair(result);
  return result;
}


bool track_hands(cv::Mat& frame_a, cv::Mat& frame_b,
                 vector<cv::Point2f>& cur_points,
                 vector<cv::Point2f>& next_points)
{
  if (cur_points.size() != 2) {
    cout << "wrong number of points" << endl;
    return false;
  }

  vector<uchar> status;
  vector<float> errors;

  sort_pair(cur_points);

  cv::calcOpticalFlowPyrLK(
    frame_a,
    frame_b,
    cur_points, next_points,
    status, errors,
    cv::Size(LK_WIN_SIZE, LK_WIN_SIZE), 3,
    cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 25, 0.03),
    0.5, 0
  );

  for (size_t i = 0; i < next_points.size(); ) {
    if (next_points[i].x < 0 || next_points[i].x >= frame_a.cols ||
        next_points[i].y < 0 || next_points[i].y >= frame_a.rows)
      next_points.erase(next_points.begin()+i);
    else
      ++i;
  }

  if (next_points.size() != 2) {
    cout << "lost a point" << endl;
    return false;
  }

  if (cv::dist2(cur_points[0], next_points[0]) >= PT_DIST2_THRESHOLD ||
      cv::dist2(cur_points[1], next_points[1]) >= PT_DIST2_THRESHOLD) {
    cout << "outlier point" << endl;
    return false;
  }

  return true;
}
